package edu.tec.mty.extras.fuzzy;

import java.util.Vector;


public class Fuzzy {
	//public static void main(String[] args){
	double[] fuzzyOutput = new double[3];
	double out1 = 0;
	double out2 = 0;
	double out3 = 0;
	FuzzyModel Robocup;
	
	//INPUT MFs (Value, Variable, Sigma, Mean, MinR, MaxR)
	InputGaussMF distanceClose = new InputGaussMF("Close", "Distance", 21.2, 0, 0, 100);
	InputGaussMF distanceFar = new InputGaussMF("Far", "Distance", 12.8, 40, 0, 40);
	
	InputGaussMF staminaLow = new InputGaussMF("Low", "StaminaX100", 1000, 0, 0, 3000);
	InputGaussMF staminaHigh = new InputGaussMF("High", "StaminaX100", 1000, 3000, 0, 3000);
	
	InputGaussMF ySpdToLeft = new InputGaussMF("ToLeft", "relYSpd", 0.128, -0.3, -0.3, 0.3);
	InputGaussMF ySpdStay = new InputGaussMF("Stay", "relYSpd", 0.128, 0, -0.5, 0.5);
	InputGaussMF ySpdToRight = new InputGaussMF("ToRight", "relYSpd", 0.128, 0.3, -0.3, 0.3);
	
	InputGaussMF xSpdSlow = new InputGaussMF("Slow", "relXSpd", 0.17, 0, 0, 0.5);
	InputGaussMF xSpdFast = new InputGaussMF("Fast", "relXSpd", 0.085, 0.3, 0, 0.3);
	
	InputGaussMF dirToOppAtLeft = new InputGaussMF("AtLeft", "dirToOpp", 19.2, -45, -45, 45);
	InputGaussMF dirToOppCloseAtLeft = new InputGaussMF("CloseAtLeft", "dirToOpp", 9.9, -15, -45, 45);
	InputGaussMF dirToOppAtCenter = new InputGaussMF("AtCenter", "dirToOpp", 9.9, 0, -45, 45);
	InputGaussMF dirToOppCloseAtRight = new InputGaussMF("CloseAtRight", "dirToOpp", 9.9, 15, -45, 45);
	InputGaussMF dirToOppAtRight = new InputGaussMF("AtRight", "dirToOpp", 19.2, 45, -45, 45);
	
	//Output MFs (Value, Variable, Sigma, Mean, minR, maxR)
	OutputGaussMF powerGoSlow = new OutputGaussMF("GoSlow", "Power", 29.8, 0, 0, 100); //CHECK
	OutputGaussMF powerGoJogging = new OutputGaussMF("GoJogging", "Power", 20, 60, 0, 100); //CHECK
	OutputGaussMF powerSprint = new OutputGaussMF("Sprint", "Power", 20, 100, 0, 100); //CHECK
	
	OutputGaussMF headingGoLeft = new OutputGaussMF("GoLeft", "Heading", 35, -90, -90, 90); //CHECK
	OutputGaussMF headingStayCenter = new OutputGaussMF("StayCenter", "Heading", 35, 0, -90, 90); //CHECK
	OutputGaussMF headingGoRight = new OutputGaussMF("GoRight", "Heading", 35, 90, -90, 90); //CHECK
	
	OutputGaussMF pHeadingStrafeLeft = new OutputGaussMF("StrafeLeft", "PowerHeading", 35, -90, -90, 90); //CHECK
	OutputGaussMF pHeadingStraight = new OutputGaussMF("Straight", "PowerHeading", 35, 0, -90, 90); //CHECK
	OutputGaussMF pHeadingStrafeRight = new OutputGaussMF("StrafeRight", "PowerHeading", 35, 90, -90, 90); //CHECK
	
	//generates general rules model
	public void leftPreference(){
		Vector<FuzzyRule> model = new Vector<FuzzyRule>();
		
		FuzzyRule rule1 = new FuzzyRule("and", distanceClose, staminaLow, ySpdToLeft, xSpdSlow, dirToOppAtLeft, 
										"and", powerGoSlow, headingStayCenter, pHeadingStraight);
		model.add(rule1); //Checked
		FuzzyRule rule2 = new FuzzyRule("and", distanceClose, staminaLow, ySpdToLeft, xSpdSlow, dirToOppCloseAtLeft, 
										"and", powerGoSlow, headingStayCenter, pHeadingStraight);
		model.add(rule2); //Checked
		FuzzyRule rule3 = new FuzzyRule("and", distanceClose, staminaLow, ySpdToLeft, xSpdSlow, dirToOppAtCenter, 
										"and", powerGoSlow, headingStayCenter, pHeadingStraight);
		model.add(rule3); //Checked
		FuzzyRule rule4 = new FuzzyRule("and", distanceClose, staminaLow, ySpdToLeft, xSpdSlow, dirToOppCloseAtRight, 
										"and", powerGoSlow, headingGoRight, pHeadingStraight);
		model.add(rule4); //Checked
		FuzzyRule rule5 = new FuzzyRule("and", distanceClose, staminaLow, ySpdToLeft, xSpdSlow, dirToOppAtRight, 
										"and", powerGoSlow, headingStayCenter, pHeadingStraight);
		model.add(rule5); //Checked
		
		FuzzyRule rule6 = new FuzzyRule("and", distanceClose, staminaLow, ySpdToLeft, xSpdFast, dirToOppAtLeft, 
										"and", powerGoSlow, headingStayCenter, pHeadingStraight);
		model.add(rule6); //Checked
		FuzzyRule rule7 = new FuzzyRule("and", distanceClose, staminaLow, ySpdToLeft, xSpdFast, dirToOppCloseAtLeft,
										"and", powerGoSlow, headingStayCenter, pHeadingStraight);
		model.add(rule7); //Checked
		FuzzyRule rule8 = new FuzzyRule("and", distanceClose, staminaLow, ySpdToLeft, xSpdFast, dirToOppAtCenter,
										"and", powerGoSlow, headingStayCenter, pHeadingStraight);
		model.add(rule8); //Checked
		FuzzyRule rule9 = new FuzzyRule("and", distanceClose, staminaLow, ySpdToLeft, xSpdFast, dirToOppCloseAtRight,
										"and", powerGoSlow, headingGoRight, pHeadingStrafeRight);
		model.add(rule9); //Checked
		FuzzyRule rule10 = new FuzzyRule("and", distanceClose, staminaLow, ySpdToLeft, xSpdFast, dirToOppAtRight,
										"and", powerGoSlow, headingStayCenter, pHeadingStraight);
		model.add(rule10); //Checked
		
		
		FuzzyRule rule11 = new FuzzyRule("and", distanceClose, staminaLow, ySpdStay, xSpdSlow, dirToOppAtLeft, 
										 "and", powerGoSlow, headingStayCenter, pHeadingStraight);
		model.add(rule11); //Checked
		FuzzyRule rule12 = new FuzzyRule("and", distanceClose, staminaLow, ySpdStay, xSpdSlow, dirToOppCloseAtLeft,
										 "and", powerGoSlow, headingGoRight, pHeadingStraight);
		model.add(rule12); //Checked
		FuzzyRule rule13 = new FuzzyRule("and", distanceClose, staminaLow, ySpdStay, xSpdSlow, dirToOppAtCenter, 
										 "and", powerGoSlow, headingGoLeft, pHeadingStraight);
		model.add(rule13); //Checked GAIN SCHEDULE
		FuzzyRule rule14 = new FuzzyRule("and", distanceClose, staminaLow, ySpdStay, xSpdSlow, dirToOppCloseAtRight,
										 "and", powerGoSlow, headingGoLeft, pHeadingStraight);
		model.add(rule14); //Checked
		FuzzyRule rule15 = new FuzzyRule("and", distanceClose, staminaLow, ySpdStay, xSpdSlow, dirToOppAtRight,
										 "and", powerGoSlow, headingStayCenter, pHeadingStraight);
		model.add(rule15); //Checked

		FuzzyRule rule16 = new FuzzyRule("and", distanceClose, staminaLow, ySpdStay, xSpdFast, dirToOppAtLeft,
										 "and", powerGoSlow, headingStayCenter, pHeadingStraight);
		model.add(rule16); //Checked
		FuzzyRule rule17 = new FuzzyRule("and", distanceClose, staminaLow, ySpdStay, xSpdFast, dirToOppCloseAtLeft,
										 "and", powerGoSlow, headingGoRight, pHeadingStrafeRight);
		model.add(rule17); //Checked
		FuzzyRule rule18 = new FuzzyRule("and", distanceClose, staminaLow, ySpdStay, xSpdFast, dirToOppAtCenter,
										 "and", powerGoSlow, headingGoLeft, pHeadingStrafeLeft);
		model.add(rule18); //Checked GAIN SCHEDULE
		FuzzyRule rule19 = new FuzzyRule("and", distanceClose, staminaLow, ySpdStay, xSpdFast, dirToOppCloseAtRight,
										 "and", powerGoSlow, headingGoLeft, pHeadingStrafeLeft);
		model.add(rule19); //Checked
		FuzzyRule rule20 = new FuzzyRule("and", distanceClose, staminaLow, ySpdStay, xSpdFast, dirToOppAtRight,
										 "and", powerGoSlow, headingStayCenter, pHeadingStraight);
		model.add(rule20); //Checked
		
		
		FuzzyRule rule21 = new FuzzyRule("and", distanceClose, staminaLow, ySpdToRight, xSpdSlow, dirToOppAtLeft, 
										 "and", powerGoSlow, headingStayCenter, pHeadingStraight);
		model.add(rule21); //Checked
		FuzzyRule rule22 = new FuzzyRule("and", distanceClose, staminaLow, ySpdToRight, xSpdSlow, dirToOppCloseAtLeft,
				 						 "and", powerGoSlow, headingGoLeft, pHeadingStraight);
		model.add(rule22); //Checked
		FuzzyRule rule23 = new FuzzyRule("and", distanceClose, staminaLow, ySpdToRight, xSpdSlow, dirToOppAtCenter,
										 "and", powerGoSlow, headingStayCenter, pHeadingStraight);
		model.add(rule23); //Checked
		FuzzyRule rule24 = new FuzzyRule("and", distanceClose, staminaLow, ySpdToRight, xSpdSlow, dirToOppCloseAtRight,
										 "and", powerGoSlow, headingStayCenter, pHeadingStraight);
		model.add(rule24); //Checked
		FuzzyRule rule25 = new FuzzyRule("and", distanceClose, staminaLow, ySpdToRight, xSpdSlow, dirToOppAtRight,
										 "and", powerGoSlow, headingStayCenter, pHeadingStraight);
		model.add(rule25); //Checked
		
		FuzzyRule rule26 = new FuzzyRule("and", distanceClose, staminaLow, ySpdToRight, xSpdFast, dirToOppAtLeft,
										 "and", powerGoSlow, headingStayCenter, pHeadingStraight);
		model.add(rule26); //Checked
		FuzzyRule rule27 = new FuzzyRule("and", distanceClose, staminaLow, ySpdToRight, xSpdFast, dirToOppCloseAtLeft,
										 "and", powerGoSlow, headingGoLeft, pHeadingStrafeLeft);
		model.add(rule27); //Checked
		FuzzyRule rule28 = new FuzzyRule("and", distanceClose, staminaLow, ySpdToRight, xSpdFast, dirToOppAtCenter,
										 "and", powerGoSlow, headingStayCenter, pHeadingStraight);
		model.add(rule28); //Checked
		FuzzyRule rule29 = new FuzzyRule("and", distanceClose, staminaLow, ySpdToRight, xSpdFast, dirToOppCloseAtRight,
										 "and", powerGoSlow, headingStayCenter, pHeadingStraight);
		model.add(rule29); //Checked
		FuzzyRule rule30 = new FuzzyRule("and", distanceClose, staminaLow, ySpdToRight, xSpdFast, dirToOppAtRight,
										 "and", powerGoSlow, headingStayCenter, pHeadingStraight);
		model.add(rule30); //Checked
		
		
		FuzzyRule rule31 = new FuzzyRule("and", distanceClose, staminaHigh, ySpdToLeft, xSpdSlow, dirToOppAtLeft, 
										 "and", powerGoJogging, headingStayCenter, pHeadingStraight);
		model.add(rule31); //Checked
		FuzzyRule rule32 = new FuzzyRule("and", distanceClose, staminaHigh, ySpdToLeft, xSpdSlow, dirToOppCloseAtLeft, 
										 "and", powerGoJogging, headingStayCenter, pHeadingStraight);
		model.add(rule32); //Checked
		FuzzyRule rule33 = new FuzzyRule("and", distanceClose, staminaHigh, ySpdToLeft, xSpdSlow, dirToOppAtCenter, 
										 "and", powerGoJogging, headingStayCenter, pHeadingStraight);
		model.add(rule33); //Checked
		FuzzyRule rule34 = new FuzzyRule("and", distanceClose, staminaHigh, ySpdToLeft, xSpdSlow, dirToOppCloseAtRight, 
										 "and", powerGoJogging, headingGoRight, pHeadingStraight);
		model.add(rule34); //Checked
		FuzzyRule rule35 = new FuzzyRule("and", distanceClose, staminaHigh, ySpdToLeft, xSpdSlow, dirToOppAtRight, 
										 "and", powerGoJogging, headingStayCenter, pHeadingStraight);
		model.add(rule35); //Checked

		FuzzyRule rule36 = new FuzzyRule("and", distanceClose, staminaHigh, ySpdToLeft, xSpdFast, dirToOppAtLeft,
										 "and", powerSprint, headingStayCenter, pHeadingStraight);
		model.add(rule36); //Checked
		FuzzyRule rule37 = new FuzzyRule("and", distanceClose, staminaHigh, ySpdToLeft, xSpdFast, dirToOppCloseAtLeft,
										 "and", powerSprint, headingStayCenter, pHeadingStraight);
		model.add(rule37); //Checked
		FuzzyRule rule38 = new FuzzyRule("and", distanceClose, staminaHigh, ySpdToLeft, xSpdFast, dirToOppAtCenter,
										 "and", powerSprint, headingStayCenter, pHeadingStraight);
		model.add(rule38); //Checked
		FuzzyRule rule39 = new FuzzyRule("and", distanceClose, staminaHigh, ySpdToLeft, xSpdFast, dirToOppCloseAtRight,
										 "and", powerSprint, headingGoRight, pHeadingStrafeRight);
		model.add(rule39); //Checked
		FuzzyRule rule40 = new FuzzyRule("and", distanceClose, staminaHigh, ySpdToLeft, xSpdFast, dirToOppAtRight,
										 "and", powerSprint, headingStayCenter, pHeadingStraight);
		model.add(rule40); //Checked
		

		FuzzyRule rule41 = new FuzzyRule("and", distanceClose, staminaHigh, ySpdStay, xSpdSlow, dirToOppAtLeft, 
				 						 "and", powerGoJogging, headingStayCenter, pHeadingStraight);
		model.add(rule41); //Checked
		FuzzyRule rule42 = new FuzzyRule("and", distanceClose, staminaHigh, ySpdStay, xSpdSlow, dirToOppCloseAtLeft,
				 						 "and", powerGoJogging, headingGoRight, pHeadingStraight);
		model.add(rule42); //Checked
		FuzzyRule rule43 = new FuzzyRule("and", distanceClose, staminaHigh, ySpdStay, xSpdSlow, dirToOppAtCenter, 
				 						 "and", powerGoJogging, headingGoLeft, pHeadingStraight);
		model.add(rule43); //Checked GAIN SCHEDULE
		FuzzyRule rule44 = new FuzzyRule("and", distanceClose, staminaHigh, ySpdStay, xSpdSlow, dirToOppCloseAtRight,
				 						 "and", powerGoJogging, headingGoLeft, pHeadingStraight);
		model.add(rule44); //Checked
		FuzzyRule rule45 = new FuzzyRule("and", distanceClose, staminaHigh, ySpdStay, xSpdSlow, dirToOppAtRight,
				 						 "and", powerGoJogging, headingStayCenter, pHeadingStraight);
		model.add(rule45); //Checked

		FuzzyRule rule46 = new FuzzyRule("and", distanceClose, staminaHigh, ySpdStay, xSpdFast, dirToOppAtLeft,
				 						 "and", powerSprint, headingStayCenter, pHeadingStraight);
		model.add(rule46); //Checked
		FuzzyRule rule47 = new FuzzyRule("and", distanceClose, staminaHigh, ySpdStay, xSpdFast, dirToOppCloseAtLeft,
				 						 "and", powerSprint, headingGoRight, pHeadingStrafeRight);
		model.add(rule47); //Checked
		FuzzyRule rule48 = new FuzzyRule("and", distanceClose, staminaHigh, ySpdStay, xSpdFast, dirToOppAtCenter,
				 						 "and", powerSprint, headingGoLeft, pHeadingStrafeLeft);
		model.add(rule48); //Checked GAIN SCHEDULE
		FuzzyRule rule49 = new FuzzyRule("and", distanceClose, staminaHigh, ySpdStay, xSpdFast, dirToOppCloseAtRight,
				 						 "and", powerSprint, headingGoLeft, pHeadingStrafeLeft);
		model.add(rule49); //Checked
		FuzzyRule rule50 = new FuzzyRule("and", distanceClose, staminaHigh, ySpdStay, xSpdFast, dirToOppAtRight,
				 						 "and", powerSprint, headingStayCenter, pHeadingStraight);
		model.add(rule50); //Checked


		FuzzyRule rule51 = new FuzzyRule("and", distanceClose, staminaHigh, ySpdToRight, xSpdSlow, dirToOppAtLeft, 
				 						 "and", powerGoJogging, headingStayCenter, pHeadingStraight);
		model.add(rule51); //Checked
		FuzzyRule rule52 = new FuzzyRule("and", distanceClose, staminaHigh, ySpdToRight, xSpdSlow, dirToOppCloseAtLeft,
					 					 "and", powerGoJogging, headingGoLeft, pHeadingStraight);
		model.add(rule52); //Checked
		FuzzyRule rule53 = new FuzzyRule("and", distanceClose, staminaHigh, ySpdToRight, xSpdSlow, dirToOppAtCenter,
				 						 "and", powerGoJogging, headingStayCenter, pHeadingStraight);
		model.add(rule53); //Checked
		FuzzyRule rule54 = new FuzzyRule("and", distanceClose, staminaHigh, ySpdToRight, xSpdSlow, dirToOppCloseAtRight,
										 "and", powerGoJogging, headingStayCenter, pHeadingStraight);
		model.add(rule54); //Checked
		FuzzyRule rule55 = new FuzzyRule("and", distanceClose, staminaHigh, ySpdToRight, xSpdSlow, dirToOppAtRight,
				 						 "and", powerGoJogging, headingStayCenter, pHeadingStraight);
		model.add(rule55); //Checked

		FuzzyRule rule56 = new FuzzyRule("and", distanceClose, staminaHigh, ySpdToRight, xSpdFast, dirToOppAtLeft,
										 "and", powerSprint, headingStayCenter, pHeadingStraight);
		model.add(rule56); //Checked
		FuzzyRule rule57 = new FuzzyRule("and", distanceClose, staminaHigh, ySpdToRight, xSpdFast, dirToOppCloseAtLeft,
				 						 "and", powerSprint, headingGoLeft, pHeadingStrafeLeft);
		model.add(rule57); //Checked
		FuzzyRule rule58 = new FuzzyRule("and", distanceClose, staminaHigh, ySpdToRight, xSpdFast, dirToOppAtCenter,
				 						 "and", powerSprint, headingStayCenter, pHeadingStraight);
		model.add(rule58); //Checked
		FuzzyRule rule59 = new FuzzyRule("and", distanceClose, staminaHigh, ySpdToRight, xSpdFast, dirToOppCloseAtRight,
										 "and", powerSprint, headingStayCenter, pHeadingStraight);
		model.add(rule59); //Checked
		FuzzyRule rule60 = new FuzzyRule("and", distanceClose, staminaHigh, ySpdToRight, xSpdFast, dirToOppAtRight,
										 "and", powerSprint, headingStayCenter, pHeadingStraight);
		model.add(rule60); //Checked
		
		
		FuzzyRule rule61 = new FuzzyRule("and", distanceFar, staminaLow,
				 						 "and", powerGoSlow, headingStayCenter, pHeadingStraight);
		model.add(rule61); //Checked
		FuzzyRule rule62 = new FuzzyRule("and", distanceFar, staminaHigh,
				 						 "and", powerGoJogging, headingStayCenter, pHeadingStraight);
		model.add(rule62); //Checked
		
		//TODO Checar limites
		FuzzyModel Robocup = new FuzzyModel(model, 0, 100, -90, 90, -90, 90);
		
		this.Robocup = Robocup;
		
	}
	
	public void rightPreference(){
		Vector<FuzzyRule> model = new Vector<FuzzyRule>();
		
		FuzzyRule rule1 = new FuzzyRule("and", distanceClose, staminaLow, ySpdToLeft, xSpdSlow, dirToOppAtLeft, 
										"and", powerGoSlow, headingStayCenter, pHeadingStraight);
		model.add(rule1); //Checked
		FuzzyRule rule2 = new FuzzyRule("and", distanceClose, staminaLow, ySpdToLeft, xSpdSlow, dirToOppCloseAtLeft, 
										"and", powerGoSlow, headingStayCenter, pHeadingStraight);
		model.add(rule2); //Checked
		FuzzyRule rule3 = new FuzzyRule("and", distanceClose, staminaLow, ySpdToLeft, xSpdSlow, dirToOppAtCenter, 
										"and", powerGoSlow, headingStayCenter, pHeadingStraight);
		model.add(rule3); //Checked
		FuzzyRule rule4 = new FuzzyRule("and", distanceClose, staminaLow, ySpdToLeft, xSpdSlow, dirToOppCloseAtRight, 
										"and", powerGoSlow, headingGoRight, pHeadingStraight);
		model.add(rule4); //Checked
		FuzzyRule rule5 = new FuzzyRule("and", distanceClose, staminaLow, ySpdToLeft, xSpdSlow, dirToOppAtRight, 
										"and", powerGoSlow, headingStayCenter, pHeadingStraight);
		model.add(rule5); //Checked
		
		FuzzyRule rule6 = new FuzzyRule("and", distanceClose, staminaLow, ySpdToLeft, xSpdFast, dirToOppAtLeft, 
										"and", powerGoSlow, headingStayCenter, pHeadingStraight);
		model.add(rule6); //Checked
		FuzzyRule rule7 = new FuzzyRule("and", distanceClose, staminaLow, ySpdToLeft, xSpdFast, dirToOppCloseAtLeft,
										"and", powerGoSlow, headingStayCenter, pHeadingStraight);
		model.add(rule7); //Checked
		FuzzyRule rule8 = new FuzzyRule("and", distanceClose, staminaLow, ySpdToLeft, xSpdFast, dirToOppAtCenter,
										"and", powerGoSlow, headingStayCenter, pHeadingStraight);
		model.add(rule8); //Checked
		FuzzyRule rule9 = new FuzzyRule("and", distanceClose, staminaLow, ySpdToLeft, xSpdFast, dirToOppCloseAtRight,
										"and", powerGoSlow, headingGoRight, pHeadingStrafeRight);
		model.add(rule9); //Checked
		FuzzyRule rule10 = new FuzzyRule("and", distanceClose, staminaLow, ySpdToLeft, xSpdFast, dirToOppAtRight,
										 "and", powerGoSlow, headingStayCenter, pHeadingStraight);
		model.add(rule10); //Checked


		FuzzyRule rule11 = new FuzzyRule("and", distanceClose, staminaLow, ySpdStay, xSpdSlow, dirToOppAtLeft,
										 "and", powerGoSlow, headingStayCenter, pHeadingStraight);
		model.add(rule11); //Checked
		FuzzyRule rule12 = new FuzzyRule("and", distanceClose, staminaLow, ySpdStay, xSpdSlow, dirToOppCloseAtLeft,
										 "and", powerGoSlow, headingGoRight, pHeadingStraight);
		model.add(rule12); //Checked
		FuzzyRule rule13 = new FuzzyRule("and", distanceClose, staminaLow, ySpdStay, xSpdSlow, dirToOppAtCenter,
										 "and", powerGoSlow, headingGoRight, pHeadingStraight);
		model.add(rule13); //Checked GAIN SCHEDULE
		FuzzyRule rule14 = new FuzzyRule("and", distanceClose, staminaLow, ySpdStay, xSpdSlow, dirToOppCloseAtRight,
										 "and", powerGoSlow, headingGoLeft, pHeadingStraight);
		model.add(rule14); //Checked
		FuzzyRule rule15 = new FuzzyRule("and", distanceClose, staminaLow, ySpdStay, xSpdSlow, dirToOppAtRight,
										 "and", powerGoSlow, headingStayCenter, pHeadingStraight);
		model.add(rule15); //Checked

		FuzzyRule rule16 = new FuzzyRule("and", distanceClose, staminaLow, ySpdStay, xSpdFast, dirToOppAtLeft,
										 "and", powerGoSlow, headingStayCenter, pHeadingStraight);
		model.add(rule16); //Checked
		FuzzyRule rule17 = new FuzzyRule("and", distanceClose, staminaLow, ySpdStay, xSpdFast, dirToOppCloseAtLeft,
										 "and", powerGoSlow, headingGoRight, pHeadingStrafeRight);
		model.add(rule17); //Checked
		FuzzyRule rule18 = new FuzzyRule("and", distanceClose, staminaLow, ySpdStay, xSpdFast, dirToOppAtCenter,
										 "and", powerGoSlow, headingGoRight, pHeadingStrafeRight);
		model.add(rule18); //Checked GAIN SCHEDULE
		FuzzyRule rule19 = new FuzzyRule("and", distanceClose, staminaLow, ySpdStay, xSpdFast, dirToOppCloseAtRight,
										 "and", powerGoSlow, headingGoLeft, pHeadingStrafeLeft);
		model.add(rule19); //Checked
		FuzzyRule rule20 = new FuzzyRule("and", distanceClose, staminaLow, ySpdStay, xSpdFast, dirToOppAtRight,
										 "and", powerGoSlow, headingStayCenter, pHeadingStraight);
		model.add(rule20); //Checked
		
		
		FuzzyRule rule21 = new FuzzyRule("and", distanceClose, staminaLow, ySpdToRight, xSpdSlow, dirToOppAtLeft, 
				 						 "and", powerGoSlow, headingStayCenter, pHeadingStraight);
		model.add(rule21); //Checked
		FuzzyRule rule22 = new FuzzyRule("and", distanceClose, staminaLow, ySpdToRight, xSpdSlow, dirToOppCloseAtLeft,
										 "and", powerGoSlow, headingGoLeft, pHeadingStraight);
		model.add(rule22); //Checked
		FuzzyRule rule23 = new FuzzyRule("and", distanceClose, staminaLow, ySpdToRight, xSpdSlow, dirToOppAtCenter,
										 "and", powerGoSlow, headingStayCenter, pHeadingStraight);
		model.add(rule23); //Checked
		FuzzyRule rule24 = new FuzzyRule("and", distanceClose, staminaLow, ySpdToRight, xSpdSlow, dirToOppCloseAtRight,
										 "and", powerGoSlow, headingStayCenter, pHeadingStraight); 
		model.add(rule24); //Checked
		FuzzyRule rule25 = new FuzzyRule("and", distanceClose, staminaLow, ySpdToRight, xSpdSlow, dirToOppAtRight,
										 "and", powerGoSlow, headingStayCenter, pHeadingStraight);
		model.add(rule25); //Checked

		FuzzyRule rule26 = new FuzzyRule("and", distanceClose, staminaLow, ySpdToRight, xSpdFast, dirToOppAtLeft,
										 "and", powerGoSlow, headingStayCenter, pHeadingStraight);
		model.add(rule26); //Checked
		FuzzyRule rule27 = new FuzzyRule("and", distanceClose, staminaLow, ySpdToRight, xSpdFast, dirToOppCloseAtLeft,
										 "and", powerGoSlow, headingGoLeft, pHeadingStrafeLeft);
		model.add(rule27); //Checked
		FuzzyRule rule28 = new FuzzyRule("and", distanceClose, staminaLow, ySpdToRight, xSpdFast, dirToOppAtCenter,
										 "and", powerGoSlow, headingStayCenter, pHeadingStraight);
		model.add(rule28); //Checked
		FuzzyRule rule29 = new FuzzyRule("and", distanceClose, staminaLow, ySpdToRight, xSpdFast, dirToOppCloseAtRight,
										 "and", powerGoSlow, headingStayCenter, pHeadingStraight);
		model.add(rule29); //Checked
		FuzzyRule rule30 = new FuzzyRule("and", distanceClose, staminaLow, ySpdToRight, xSpdFast, dirToOppAtRight,
										 "and", powerGoSlow, headingStayCenter, pHeadingStraight);
		model.add(rule30); //Checked
		
		FuzzyRule rule31 = new FuzzyRule("and", distanceClose, staminaHigh, ySpdToLeft, xSpdSlow, dirToOppAtLeft, 
				 						 "and", powerGoJogging, headingStayCenter, pHeadingStraight);
		model.add(rule31); //Checked
		FuzzyRule rule32 = new FuzzyRule("and", distanceClose, staminaHigh, ySpdToLeft, xSpdSlow, dirToOppCloseAtLeft,
								 		 "and", powerGoJogging, headingStayCenter, pHeadingStraight);
		model.add(rule32); //Checked
		FuzzyRule rule33 = new FuzzyRule("and", distanceClose, staminaHigh, ySpdToLeft, xSpdSlow, dirToOppAtCenter,
								 		 "and", powerGoJogging, headingStayCenter, pHeadingStraight);
		model.add(rule33); //Checked
		FuzzyRule rule34 = new FuzzyRule("and", distanceClose, staminaHigh, ySpdToLeft, xSpdSlow, dirToOppCloseAtRight,
								 		 "and", powerGoJogging, headingGoRight, pHeadingStraight);
		model.add(rule34); //Checked
		FuzzyRule rule35 = new FuzzyRule("and", distanceClose, staminaHigh, ySpdToLeft, xSpdSlow, dirToOppAtRight,
										 "and", powerGoJogging, headingStayCenter, pHeadingStraight);
		model.add(rule35); //Checked
		
		FuzzyRule rule36 = new FuzzyRule("and", distanceClose, staminaHigh, ySpdToLeft, xSpdFast, dirToOppAtLeft,
				 						 "and", powerSprint, headingStayCenter, pHeadingStraight);
		model.add(rule36); //Checked
		FuzzyRule rule37 = new FuzzyRule("and", distanceClose, staminaHigh, ySpdToLeft, xSpdFast, dirToOppCloseAtLeft,
										 "and", powerSprint, headingStayCenter, pHeadingStraight);
		model.add(rule37); //Checked
		FuzzyRule rule38 = new FuzzyRule("and", distanceClose, staminaHigh, ySpdToLeft, xSpdFast, dirToOppAtCenter,
										 "and", powerSprint, headingStayCenter, pHeadingStraight);
		model.add(rule38); //Checked
		FuzzyRule rule39 = new FuzzyRule("and", distanceClose, staminaHigh, ySpdToLeft, xSpdFast, dirToOppCloseAtRight,
										 "and", powerSprint, headingGoRight, pHeadingStrafeRight);
		model.add(rule39); //Checked
		FuzzyRule rule40 = new FuzzyRule("and", distanceClose, staminaHigh, ySpdToLeft, xSpdFast, dirToOppAtRight,
										 "and", powerSprint, headingStayCenter, pHeadingStraight);
		model.add(rule40); //Checked
		

		FuzzyRule rule41 = new FuzzyRule("and", distanceClose, staminaHigh, ySpdStay, xSpdSlow, dirToOppAtLeft,
										 "and", powerGoJogging, headingStayCenter, pHeadingStraight);
		model.add(rule41); //Checked
		FuzzyRule rule42 = new FuzzyRule("and", distanceClose, staminaHigh, ySpdStay, xSpdSlow, dirToOppCloseAtLeft,
										 "and", powerGoJogging, headingGoRight, pHeadingStraight);
		model.add(rule42); //Checked
		FuzzyRule rule43 = new FuzzyRule("and", distanceClose, staminaHigh, ySpdStay, xSpdSlow, dirToOppAtCenter,
										 "and", powerGoJogging, headingGoRight, pHeadingStraight);
		model.add(rule43); //Checked GAIN SCHEDULE
		FuzzyRule rule44 = new FuzzyRule("and", distanceClose, staminaHigh, ySpdStay, xSpdSlow, dirToOppCloseAtRight,
				 						 "and", powerGoJogging, headingGoLeft, pHeadingStraight);
		model.add(rule44); //Checked
		FuzzyRule rule45 = new FuzzyRule("and", distanceClose, staminaHigh, ySpdStay, xSpdSlow, dirToOppAtRight,
										 "and", powerGoJogging, headingStayCenter, pHeadingStraight);
		model.add(rule45); //Checked

		FuzzyRule rule46 = new FuzzyRule("and", distanceClose, staminaHigh, ySpdStay, xSpdFast, dirToOppAtLeft,
										 "and", powerSprint, headingStayCenter, pHeadingStraight);
		model.add(rule46); //Checked
		FuzzyRule rule47 = new FuzzyRule("and", distanceClose, staminaHigh, ySpdStay, xSpdFast, dirToOppCloseAtLeft,
										 "and", powerSprint, headingGoRight, pHeadingStrafeRight);
		model.add(rule47); //Checked
		FuzzyRule rule48 = new FuzzyRule("and", distanceClose, staminaHigh, ySpdStay, xSpdFast, dirToOppAtCenter,
				 						 "and", powerSprint, headingGoRight, pHeadingStrafeRight);
		model.add(rule48); //Checked GAIN SCHEDULE
		FuzzyRule rule49 = new FuzzyRule("and", distanceClose, staminaHigh, ySpdStay, xSpdFast, dirToOppCloseAtRight,
										 "and", powerSprint, headingGoLeft, pHeadingStrafeLeft);
		model.add(rule49); //Checked
		FuzzyRule rule50 = new FuzzyRule("and", distanceClose, staminaHigh, ySpdStay, xSpdFast, dirToOppAtRight,
										 "and", powerSprint, headingStayCenter, pHeadingStraight);
		model.add(rule50); //Checked
		

		FuzzyRule rule51 = new FuzzyRule("and", distanceClose, staminaHigh, ySpdToRight, xSpdSlow, dirToOppAtLeft,
										 "and", powerGoJogging, headingStayCenter, pHeadingStraight);
		model.add(rule51); //Checked
		FuzzyRule rule52 = new FuzzyRule("and", distanceClose, staminaHigh, ySpdToRight, xSpdSlow, dirToOppCloseAtLeft,
										 "and", powerGoJogging, headingGoLeft, pHeadingStraight);
		model.add(rule52); //Checked
		FuzzyRule rule53 = new FuzzyRule("and", distanceClose, staminaHigh, ySpdToRight, xSpdSlow, dirToOppAtCenter,
										 "and", powerGoJogging, headingStayCenter, pHeadingStraight);
		model.add(rule53); //Checked
		FuzzyRule rule54 = new FuzzyRule("and", distanceClose, staminaHigh, ySpdToRight, xSpdSlow, dirToOppCloseAtRight,
										 "and", powerGoJogging, headingStayCenter, pHeadingStraight);
		model.add(rule54); //Checked
		FuzzyRule rule55 = new FuzzyRule("and", distanceClose, staminaHigh, ySpdToRight, xSpdSlow, dirToOppAtRight,
										 "and", powerGoJogging, headingStayCenter, pHeadingStraight);
		model.add(rule55); //Checked
		
		FuzzyRule rule56 = new FuzzyRule("and", distanceClose, staminaHigh, ySpdToRight, xSpdFast, dirToOppAtLeft,
										 "and", powerSprint, headingStayCenter, pHeadingStraight);
		model.add(rule56); //Checked
		FuzzyRule rule57 = new FuzzyRule("and", distanceClose, staminaHigh, ySpdToRight, xSpdFast, dirToOppCloseAtLeft,
										 "and", powerSprint, headingGoLeft, pHeadingStrafeLeft);
		model.add(rule57); //Checked
		FuzzyRule rule58 = new FuzzyRule("and", distanceClose, staminaHigh, ySpdToRight, xSpdFast, dirToOppAtCenter,
										 "and", powerSprint, headingStayCenter, pHeadingStraight);
		model.add(rule58); //Checked
		FuzzyRule rule59 = new FuzzyRule("and", distanceClose, staminaHigh, ySpdToRight, xSpdFast, dirToOppCloseAtRight,
										 "and", powerSprint, headingStayCenter, pHeadingStraight);
		model.add(rule59); //Checked
		FuzzyRule rule60 = new FuzzyRule("and", distanceClose, staminaHigh, ySpdToRight, xSpdFast, dirToOppAtRight,
										 "and", powerSprint, headingStayCenter, pHeadingStraight);
		model.add(rule60); //Checked
		
		
		FuzzyRule rule61 = new FuzzyRule("and", distanceFar, staminaLow,
										 "and", powerGoSlow, headingStayCenter, pHeadingStraight);
		model.add(rule61); //Checked
		FuzzyRule rule62 = new FuzzyRule("and", distanceFar, staminaHigh,
										 "and", powerGoJogging, headingStayCenter, pHeadingStraight);
		model.add(rule62); //Checked
		
		//TODO Checar limites
		FuzzyModel Robocup = new FuzzyModel(model, 0, 100, -90, 90, -90, 90);
		
		this.Robocup = Robocup;

	}

	public double[] getOutput(double distanceToOpp, double pStam,
			double relYSpd, double relXSpd, double dirToOpp) {
		
		out1 = this.Robocup.getOut1(distanceToOpp, pStam, relYSpd, relXSpd, dirToOpp);
		out2 = this.Robocup.getOut2(distanceToOpp, pStam, relYSpd, relXSpd, dirToOpp);
		out3 = this.Robocup.getOut3(distanceToOpp, pStam, relYSpd, relXSpd, dirToOpp);
		
		fuzzyOutput[0]=out1;
		fuzzyOutput[1]=out2;
		fuzzyOutput[2]=out3;
		
		return fuzzyOutput;
	}
	
}
